/**
 * @file full_graph_mapping_test.cc
 * @author DustinKsi (dustinksi@126.com)
 * @brief
 * @version 0.1
 * @date 2019-12-13
 *
 * @copyright Copyright (c) 2019
 *
 */

#include "gmock/gmock.h"
#include "gtest/gtest.h"

#include "venus/mapping/full_graph_mapping/full_graph_mapping.h"

namespace VEnus {
namespace Mapping {

TEST(FullGraphMappingTest, GenerateGraphAndHeap) {
  MappingOptions options;
  options.set_distance_ceil_threshold(30.0);  // 5.0m

  FullGraphMapping map_handler_;
  map_handler_.SetMappingOptions(options);

  unordered_map<int, pair<double, double> > fpts_list;
  fpts_list[0] = make_pair(0.0, 0.0);
  fpts_list[1] = make_pair(8.0, 0.0);
  fpts_list[2] = make_pair(5, 6);
  fpts_list[3] = make_pair(-3.0, 4.0);

  // keypoints edge distance dictionary
  map<pair<int, int>, double> _distance_dict;
  // edge's heap sort by length
  priority_queue<FullGraphMapping::EdgeWithLen> _heap;
  // keypoint's pair graph
  unordered_map<int, int> _graph;

  map_handler_.GenerateListGraphAndHeap(fpts_list, _distance_dict, _heap, _graph);

  CHECK_EQ(_heap.size(), 6) << "edges heap's size should be 6. ";
  while (!_heap.empty()) {
    auto eg = _heap.top();
    D_INFO << "[" << eg.less_id << ", " << eg.greator_id << "] : " << eg.edge_length;
    _heap.pop();
  }
}

TEST(FullGraphMappingTest, GraphMatch) {
  MappingOptions options;
  options.set_distance_ceil_threshold(12.0);
  options.set_distance_match_tolerance(0.05);

  FullGraphMapping map_handler;
  map_handler.SetMappingOptions(options);

  map_handler._g_fpts_list[0] = make_pair(-3.0, 0.0);
  map_handler._g_fpts_list[1] = make_pair(-2.7, 7.0);
  map_handler._g_fpts_list[2] = make_pair(-2.5, -3.0);
  map_handler._g_fpts_list[3] = make_pair(5.0, 6.0);
  map_handler._g_fpts_list[4] = make_pair(6.0, 2.0);
  map_handler._g_fpts_list[5] = make_pair(10.0, 1.2);

  // keypoints edge distance dictionary
  map<pair<int, int>, double> _distance_dict;
  // edge's heap sort by length
  priority_queue<FullGraphMapping::EdgeWithLen> _heap;
  // keypoint's pair graph
  unordered_map<int, int> _graph;

  map_handler.GenerateListGraphAndHeap(map_handler._g_fpts_list, map_handler._g_distance_dict,
                                       map_handler._g_edges_heap, map_handler._g_graph);

  while (!_heap.empty()) {
    auto eg = _heap.top();
    D_INFO << "[" << eg.less_id << ", " << eg.greator_id << "] : " << eg.edge_length;
    _heap.pop();
  }

  cv::Mat graph_image(480, 640, CV_8UC3, cv::Scalar(255, 255, 255));
  map_handler.GenerateGraphImage(map_handler._g_fpts_list, map_handler._g_distance_dict,
                                 graph_image);

  unordered_map<int, pair<double, double> > fpts_list;
  fpts_list[0] = make_pair(-2.0, 4);
  fpts_list[1] = make_pair(-2.3, -3);
  fpts_list[2] = make_pair(6.7, -1);
  // fpts_list[3] = make_pair(4.3, 3);

  unordered_map<int, double> score_dict;
  map_handler.ExtractKeyEdge(fpts_list, score_dict);

  map_handler.GenerateScoreGraphImage(map_handler._g_fpts_list, score_dict, graph_image);

  cv::imwrite(
      "/home/dexin/code/space_ros/dustin_ros/src/venus/venus/mapping/full_graph_mapping/"
      "easy_picture.png",
      graph_image);
}

}  // namespace Mapping
}  // namespace VEnus

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  FLAGS_stderrthreshold = google::INFO;
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}